#include "lqr.h"
#include <cmath>
using namespace arma;
using namespace std;

int main(int argc, char const *argv[])
{
  double t0 = 0, t1 = 50;
  mat A(2,2), B(2,1), Q(2,2), S(2,2), R(1,1), x0(2,1);
  
  A << 0 << 1 << endr
    << -1 << 1 << endr;
    
  B << 0 << endr << 1 << endr;
    
  Q <<  3 << 0 << endr
    <<   0 << 1 << endr;

  R << 1 << endr;

  S << 2 << 0 << endr
    << 0 << 1 << endr;

  x0 << 5 << endr << 0 << endr;

  lqrSolver mylqr1;
  mylqr1.ezset(A,B,S,Q,R,t0,t1,x0, 0.01);
  mylqr1.solve();
  mylqr1.saveData("LTI.csv");
  mylqr1.draw("LTI");


  printf("Mission one complete ^_^\n");


  // for(int i=1; i<51; i++)
  // {
  //   for (int j=1; j<101; j++)
  //   {
  //     float tf = 0.5*i;
  //     float tau = 0.001*j;
  //     unsigned int length = round((tf-t0)/tau)+1; 
  //     mylqr1.setTime(t0,tf);
  //     mylqr1.setStep(tau);
  //     mylqr1.solve();
  //     char buf[100];
  //     sprintf(buf,"/mnt/hgfs/ExchangeFile/data/model1-t%.1f-dt%0.3f.csv",tf,tau);
  //     // printf("writing %s...\t length = %u \n", buf, length);
  //     mylqr1.saveData(buf);
  //   }    
  // }
  
  //-----------------------------------------------------------
  
  // double t0 = 0, t1 = 0.5;
  // mat A(3,3), B(3,1), Q(3,3), S(3,3), R(1,1), x0(3,1);
  
  // A << -6 << -6 << -17 << endr
  //   << -1 <<  0 <<    1 << endr
  //   <<  1 <<  0 <<    0 << endr;
    
  // B << 0 << endr << 2 << endr << 1 << endr;
    
  // Q <<  1 <<  2 << 0 << endr
  //   <<  2 <<  8 << 0 << endr
  //   <<  0 <<  0 << 4 << endr;

  // R << 1 << endr;

  // S << 1 << 0 << 0 << endr
  //   << 0 << 3 << 0 << endr
  //   << 0 << 0 << 8 << endr;

  // x0 << 5 << endr << 0 << endr << 1 << endr;

  // lqrSolver mylqr1;
  // mylqr1.ezset(A,B,S,Q,R,t0,t1,x0, 0.01);
  // mylqr1.solve();
  // mylqr1.saveData("model1.dat");
  // mylqr1.draw("model1");

  
  //-----------------------------------------------------------

  t0 = 0.0; t1 = 50.0;
  MatFunction A_t(2,2),B_t(2,1),S_t(2,2), Q_t(2,2),R_t(1,1) ;
  // vectorized by row;
  A_t.pfuns[0] = [](double t){return exp(-t);};
  A_t.pfuns[1] = [](double t){return 1.0-1/sqrt(1*2*3.14)*exp(-t*t/2);};
  A_t.pfuns[2] = [](double t){return -1.0+exp(-3*t)*sin(t);};
  A_t.pfuns[3] = [](double t){return 1.0+5*exp(-t*t);};

  B_t.pfuns[0] = [](double t){return 0.0;};
  B_t.pfuns[1] = [](double t){return 1.0;};

  S_t.pfuns[0] = [](double t){return 2.0;};
  S_t.pfuns[1] = [](double t){return 0.0;};
  S_t.pfuns[2] = [](double t){return 0.0;};
  S_t.pfuns[3] = [](double t){return 1.0;};

  Q_t.pfuns[0] = [](double t){return 3.0;};
  Q_t.pfuns[1] = [](double t){return 0.0;};
  Q_t.pfuns[2] = [](double t){return 0.0;};
  Q_t.pfuns[3] = [](double t){return 1.0;};

  R_t.pfuns[0] = [](double t){return 1.0;};

  mat x_0(2,1);
  x_0 << 5 << endr << 0 << endr;

  lqrSolver mylqr2;
  mylqr2.setA_t(A_t);
  mylqr2.setB_t(B_t);
  mylqr2.setS_t(S_t);
  mylqr2.setQ_t(Q_t);
  mylqr2.setR_t(R_t);
  mylqr2.setX0(x_0);
  mylqr2.setType(TIME_VARIANT_FINITE);
  mylqr2.setTime(t0,t1);
  mylqr2.setStep(0.01);
  mylqr2.solve();
  mylqr2.saveData("LTV.csv");
  mylqr2.draw("LTV");
  printf("Mission two complete ^_^\n");
  // //-----------------------------------------------------------
  
  // R_t.pfuns[0] = [](double t){return 0.1;};
  // mylqr2.setR_t(R_t);
  // mylqr2.solve();
  // mylqr2.saveData("model2-2.csv");
  // mylqr2.draw("model2-2");

  // //-----------------------------------------------------------
  
  // mylqr2.setTime(0,10);
  // R_t.pfuns[0] = [](double t){return 1.0;};
  // mylqr2.setR_t(R_t);
  // mylqr2.solve();
  // mylqr2.saveData("model2-3.csv");
  // mylqr2.draw("model2-3");

  // //-----------------------------------------------------------

  // double t03 = 0, t13 = 10;
  // mat A3(2,2), B3(2,1), Q3(2,2), R3(1,1), x03(2,1);
  
  // A3 << 1 << 1 << endr
  //   << 0 << 1 << endr;
    
  // B3 << 0 << endr << 1 << endr;
    
  // Q3 <<  100 <<  0 << endr
  //    <<  0 <<  1 << endr;

  // R3 << 1 << endr;

  // x03 << 1 << endr << 0 << endr;

  // lqrSolver mylqr3;
  // mylqr3.ezset(A3,B3,Q3,R3,t03,t13,x03, 0.01);
  // mylqr3.solve();
  // mylqr3.saveData("model3-1.csv");
  // mylqr3.draw("model3-1");

  // //-----------------------------------------------------------
  
  // Q3(0,0)=1;
  // mylqr3.setQ(Q3);
  // mylqr3.solve();
  // mylqr3.saveData("model3-2.csv");
  // mylqr3.draw("model3-2");

  // //-----------------------------------------------------------
  
  // Q3(0,0)=0.01;
  // mylqr3.setQ(Q3);
  // mylqr3.solve();
  // mylqr3.saveData("model3-3.csv");
  // mylqr3.draw("model3-3");
  return 0;
}